package utilities;


import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.InputStreamReader;
import java.io.PrintWriter;
import java.io.IOException;
import java.util.*;
import java.net.*;
import java.lang.reflect.*;

import roombacomm.RoombaCommSerial;
import servers.Dragon;


public class RoombaEngine extends Engine {

	private roombacomm.RoombaCommSerial rc;
	private String portName; // = args[0];  // e.g. "/dev/cu.KeySerial1"    
    static boolean debug = false;
    static boolean hwhandshake = false;
    static boolean flush = false;
    private String lastSensorData="init";
    private Map lastSensorDataMap=new TreeMap();
    private Map currentSensorDataMap=new TreeMap();
	public RoombaEngine()
	{
		
	}
	
	public RoombaEngine(String name, BufferedWriter log, List actionList,
			Object configData, int sleeps, EngineCommander commander) {
		
		super(name, log, actionList, configData, sleeps, commander);
	}

	public void queueSensor(String sensorStr)
	{
		try {
			log.write("Queue sensor " + sensorStr + "\n");
			servers.Dragon.sensorDelegator(sensorStr);
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	private void processSensorData(String sensorData)
	{
		if(sensorData==null) return;
		if(lastSensorData.equals(sensorData))
		{
			return;
		}
		//must parse and queue pieces
		String pairs[] = sensorData.split(" ");
		currentSensorDataMap = new TreeMap();
		String name="";
		String value="";
		String lastValue="";
		for(int i=0;i<pairs.length;i++)
		{
			String nvPair[]=pairs[i].split(":");
			name=nvPair[0];
			value=nvPair[1];
			currentSensorDataMap.put(name, value);
			if(lastSensorDataMap.containsKey(name))
			{
				//get last value...
				if(!value.equals(lastSensorDataMap.get(name).toString()))
				{
					queueSensor(pairs[i]);
				}
			}
			else
			{
				queueSensor(pairs[i]);
			}
		}		
		lastSensorData = sensorData;
		lastSensorDataMap = currentSensorDataMap;
	}
	
	public void run()
	{
		try
		{
			Action action;
		active = true;
		int actSize = 0;
		while(running)
			{
				if(active)
				{
					//get all actions available, process each one
					actSize =workQueue.size();
					for(int i=0; i< actSize;i++)
					{
						//process each action
						 action = (Action) workQueue.get(i);
						 
						 log.write("Processing action: " + action.getActionDef().getName() + "\n");
						 
						 this.processAction(action);						
					}
					for(int i=0; i< actSize;i++)
					{
						//remove action
						workQueue.remove(0);
					}					
				}	
				
			//get sensor data and queue if necessary.............
				
//this is hte command that fails!!!!!!!!!!!!!!
            //rc.updateSensors();            
			//String sendata = rc.sensorsAsString();
			//System.out.println(sendata);
            
            //processSensorData(rc.sensorsAsString());
			
			Thread.sleep(sleeps);

   		  }
		}
		catch(Exception e)
		{
			try
			{
			log.write("Thread died: " + name + "\n" + e.getMessage());
			e.printStackTrace(new PrintWriter(log));
			}catch(Exception ee) {}
		}
	}
	
	
	public void newEngine(String name, BufferedWriter log, List workQueue, 
			Object configData
			, int sleeps, int debug, EngineCommander commander)
	{		
		super.newEngine(name, log, workQueue, configData, sleeps, debug, commander);
		

		
		portName = this.getCommander().getConfig1();
        
		try
		{
			rc = new RoombaCommSerial();
	
	        if( ! rc.connect( portName ) ) {
	            log.write("Couldn't connect to "+portName + "\n");
	            System.exit(1);
	        }      
	
	        log.write("Roomba startup on port " + portName + "\n");
	        rc.startup();
	        rc.control();
	        //rc.setSpeed(50);
	        //s
	        //rc.full();
	        //rc.setSensorsAutoUpdate(true);
	        rc.pause(30);
		}
		catch(Exception e)
		{
			
	        try {
				log.write("Errored in newEng " + e.getMessage());
			} catch (IOException e1) {
				// TODO Auto-generated catch block
				e1.printStackTrace();
			}
		
		}
	}
	
	public void playNote(int frequency, int duration)
	{
	     rc.playNote(frequency, duration);  // C	      
	}
	public void sendDrive(int forward, int distance)
	{
		if(forward==0)
		{
			rc.goForward(distance);		
		}
		else
		{
			rc.goBackward(distance);
		}
	}
	public void sendTurn(int right, int angle)
	{
		if(right==0)
		{
			rc.spinLeft(angle);		
		}
		else
		{
			rc.spinRight(angle);
		}
	}

	
	public void rStart()
	{
        rc.startup();
        rc.control();
        rc.playNote(70, 10);  // C	      		        
	}
	public void full()
	{
		rc.full();
        rc.playNote(70, 20);  // C	      
	}
	public void safe()
	{
		rc.safe();
        rc.playNote(70, 10);  // C	      		
	}
	
	public void goForward()
	{
		rc.goForward();
	}
	public void goBackward()
	{
		rc.goBackward();
	}
	public void goRight()
	{
		rc.spinRight(45);
	}
	public void goLeft()
	{
		rc.spinLeft(45);
	}
	public void rStop()
	{
		rc.stop();
	}

	public void wiggle()
	{
		int velocity=40;
		int radius=30;
		int waittime=200;
        for( int i=0; i<4; i++ ) {
            rc.drive( velocity, radius );
            rc.pause(waittime);
            rc.drive( velocity, -radius );
            rc.pause(waittime);
        }
        rc.stop();
	}
	
 	public void flash()
	{
        rc.setLEDs( true,true,true, true,true,true, 255, 255 );
        rc.pause(300);
        rc.setLEDs( false,false,false, false,false,false, 0, 128);
	}
	
	public void getSensors()
	{
		 String s = "";
		 
	        if( rc.updateSensors() )
	            s=rc.sensorsAsString();
	        else 
	            s="couldn't read Roomba. Is it connected?";
		 
	        try {
				log.write(s + "\n");
				System.out.print(s);
			} catch (IOException e1) {
				// TODO Auto-generated catch block
				e1.printStackTrace();
			}
	}
	
	
	public void close()
	{		
		//close down the roomba config	      
		try {
			running=false;
			Thread.sleep(300);
	        rc.disconnect();
			log.write("Stopping Engine\n");
			log.close();
		} catch (Exception e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	
	
	
	
	public void runProgram(String url)
	{
		try
		{
			log.write("Running " + url + "\n");
			List cmds = new ArrayList();
	    	URL yahoo = new URL(url);
	        URLConnection yc = yahoo.openConnection();
	        BufferedReader in = new BufferedReader(
	                                new InputStreamReader(
	                                yc.getInputStream()));
	        
		        String inputLine="";
		        
	  	    while ((inputLine = in.readLine()) != null) 
	  	    {	
	  	    	
	  	    	Dragon.programDelegator(inputLine);
	  	    }
	        in.close();
		}
		catch(Exception e)
		{
			e.printStackTrace();
		}
		
	}
	
	
	
	
	
	
}
